"""
Basic class of CAV
"""
from src.simulation_system.v2x_manager import V2XManager
from src.sumo_simulation.sumo_manager import SumoSimulation as Sumo


class Vehicle:
    """
    A class manager to embed different modules with vehicle together.

    Parameters
    ----------
    cav_world : simulation world object
        CAV World. This is used for V2X communication simulation.

    Attributes
    ----------
    v2x_manager : simulation v2x object
        The current V2X manager.
    """

    def __init__(self, vehicle_id, cav_world, platoon):

        self.vehicle_id = vehicle_id
        self.cav_world = cav_world
        self.platoon = platoon
        # v2x module
        if platoon:
            self.v2x_manager = V2XManager(vehicle_id, cav_world)
            self.platoon_id = None

        self._type = Sumo.get_vehicle_type(vehicle_id)
        self._length = Sumo.get_vehicle_length(vehicle_id)
        self._position = Sumo.get_vehicle_position(vehicle_id)
        self._speed = Sumo.get_vehicle_speed(vehicle_id)
        self._lane_id = Sumo.get_vehicle_lane_id(vehicle_id)
        self._lane_index = Sumo.get_vehicle_lane_index(self.vehicle_id)
        self._lane_position = Sumo.get_vehicle_lane_position(vehicle_id)
        self._road_id = Sumo.get_vehicle_road_id(vehicle_id)
        self._acceleration = Sumo.get_vehicle_acceleration(vehicle_id)
        self._rotation = Sumo.get_vehicle_rotation(vehicle_id)

    def update_info(self):
        self._position = Sumo.get_vehicle_position(self.vehicle_id)
        self._speed = Sumo.get_vehicle_speed(self.vehicle_id)
        self._lane_id = Sumo.get_vehicle_lane_id(self.vehicle_id)
        self._lane_index = Sumo.get_vehicle_lane_index(self.vehicle_id)
        self._lane_position = Sumo.get_vehicle_lane_position(self.vehicle_id)
        self._road_id = Sumo.get_vehicle_road_id(self.vehicle_id)
        if self._type == "cav" and self.platoon:
            self.v2x_manager.update_info()

    def run_step(self):
        if self.platoon:
            if self._type == "cav":
                # 检查附近是否有编队存在
                nearby_cav = self.v2x_manager.get_nearby_cav()
                if nearby_cav:
                    in_platoon_cav = dict()
                    single_cav = []
                    for vehicle_id, pid in nearby_cav.items():
                        if pid is None:
                            single_cav.append(vehicle_id)
                        else:
                            in_platoon_cav[vehicle_id] = pid
                    # 尝试加入已有的编队
                    for vehicle_id, pid in in_platoon_cav.items():
                        if self.platoon_id is None:
                            self.v2x_manager.request_join(self.vehicle_id, pid)
                # 如果没有加入任何编队，则尝试创建新的编队
                if self.platoon_id is None:
                    self.create_platoon()

    def set_leader(self):
        Sumo.set_fixed_lane(self.vehicle_id)
        # Sumo.set_acc(self.vehicle_id)
        Sumo.set_speed_factor(self.vehicle_id, 1)

    def set_member(self):
        vid = self.vehicle_id
        # 要求车辆保持在固定车道移动
        Sumo.set_fixed_lane(vid)
        # Sumo.set_cacc(vid)
        # min_gap = Sumo.get_vehicle_min_gap(self.vehicle_id)
        # # The CACC controller tries to keep (desired_gap + min_gap)
        # # meters as intra-platoon spacing
        # Sumo.set_path_cacc_parameters(vid, min_gap)
        Sumo.set_speed_factor(vid, 1.2)

    def get_vehicle_id(self):
        return self.vehicle_id

    def set_platoon_id(self, platoon_id):
        self.platoon_id = platoon_id

    def get_platoon_id(self):
        return self.platoon_id

    def create_platoon(self):
        self.cav_world.add_platoon(self)

    def set_color(self, color):
        Sumo.set_vehicle_color(self.vehicle_id, color)

    def get_type(self):
        return self._type

    def get_len(self):
        return self._length

    def get_pos(self):
        return self._position

    def get_rotation(self):
        return self._rotation

    def get_speed(self):
        return self._speed

    def get_lane_id(self):
        return self._lane_id

    def get_lane_index(self):
        return self._lane_index

    def get_lane_pos(self):
        return self._lane_position

    def get_road_id(self):
        return self._road_id

    def get_route(self):
        return Sumo.get_vehicle_route(self.vehicle_id)

    def get_nearby_cav(self):
        return self.v2x_manager.get_nearby_cav()

    def get_driving_distance(self, next_edge, pos):
        return Sumo.get_driving_distance(self.vehicle_id, next_edge, pos)
